//
// Created by ZhaoXiaoFei on 2022/8/18.
//

#include "back_end/back_end.hpp"

LocalizationBackEnd::LocalizationBackEnd(const double acc_noise, const double gyr_noise,
                    const double acc_bias_noise, const double gyr_bias_noise){
    state.pose_.block<3,3>(0 ,0).setIdentity();
    state.pose_.block<3,1>(0 ,3).setZero();
    state.vel_.setZero();
    state.acc_bias_.setZero();
    state.gyr_bias_.setZero();
    state.X_.setZero();
    state.P_.setZero();
    state.P_.block<3,3>(0,0) = 10.0 * Eigen::Matrix3d::Identity();		//position cov
    state.P_.block<3,3>(3,3) = 5.0 * Eigen::Matrix3d::Identity();		//velocity cov
    state.P_.block<3,3>(6,6) = 0.04 * Eigen::Matrix3d::Identity();		//roll-pitch-yaw cov
    state.P_.block<3,3>(9,9) = 0.0004 * Eigen::Matrix3d::Identity();	//acceleration bias cov
    state.P_.block<3,3>(12,12) = 0.0004 * Eigen::Matrix3d::Identity();	//gyroscope bias cov
    //初始化LLA 姿态
    initializer_ = std::make_unique<Initializer>(10, &state);
    Eigen::Vector3d G{0, 0, -9.81};
    imuPredict_ = std::make_unique<ImuPredict>(acc_noise, gyr_noise, acc_bias_noise, gyr_bias_noise, G);
    gpsCorrect_ = std::make_unique<GpsCorrect>();
}

void LocalizationBackEnd::processImuData(ImuData imu_data){
    if(!initializer_->isInit()){
        initializer_->Imu_initialize(imu_data);
        return;
    }
//        LOG(INFO) << " P: " << state.pose_.block<3, 1>(0, 3).transpose() << std::endl;
    imuPredict_->predict(imu_data, &state);
}

void LocalizationBackEnd::processGpsData(GpsPositionData gps_data){
    if(!initializer_->isInit()){
        initializer_->Gps_initialize(gps_data);
        return;
    }
    gpsCorrect_->correct(gps_data, &state);
    LOG(INFO) << " P: " << state.pose_.block<3, 1>(0, 3).transpose() << std::endl;
    state.X_.setZero();
}

void LocalizationBackEnd::processMagData(MagData mag_data){
    if(!initializer_->isInit()){
        initializer_->Mag_initialize(mag_data);
        return;
    }
}

State* LocalizationBackEnd::getState(){
    return &state;
}
geometry_msgs::Pose LocalizationBackEnd::getFusedPose(){
    geometry_msgs::Pose pose;

    pose.position.x = state.pose_(0,3);
    pose.position.y = state.pose_(1,3);
    pose.position.z = state.pose_(2,3);

    Eigen::Quaterniond q(state.pose_.block<3,3>(0,0));
    pose.orientation.x = q.x();
    pose.orientation.y = q.y();
    pose.orientation.z = q.z();
    pose.orientation.w = q.w();

    return pose;
}
sensor_msgs::NavSatFix LocalizationBackEnd::getFusedFix(){
    sensor_msgs::NavSatFix fusedFix;
    Eigen::Vector3d fused_lla;
//        ConvertENUToLLA(state.initLLA, state.G_p_I, &fused_lla);

    GeographicLib::LocalCartesian local_cartesian;
    local_cartesian.Reset(state.initLLA(0), state.initLLA(1), state.initLLA(2));
    local_cartesian.Reverse(state.pose_(0,3), state.pose_(1,3), state.pose_(2,3),
                            fused_lla.data()[0], fused_lla.data()[1], fused_lla.data()[2]);

    fusedFix.header.frame_id = "world";
    fusedFix.header.stamp = ros::Time::now();
    fusedFix.latitude = fused_lla[0];
    fusedFix.longitude = fused_lla[1];
    fusedFix.altitude = fused_lla[2];
    return fusedFix;
}
nav_msgs::Odometry LocalizationBackEnd::getFusedOdometry(){
    nav_msgs::Odometry odom;

    odom.header.frame_id = "world";
    odom.header.stamp = ros::Time::now();
    odom.child_frame_id = "XIMU";

    odom.pose.pose = getFusedPose();

    Eigen::Vector3d I_v = state.pose_.block<3,3>(0,0).transpose()
                          * state.vel_;//global velocity to local velocity
    //Eigen::Vector3d I_w = ; NO angular velocity estimation

    //Note: twist message is specified in frame given by "child_frame_id", i.e. XIMU.
    odom.twist.twist.linear.x = I_v[0];
    odom.twist.twist.linear.y = I_v[1];
    odom.twist.twist.linear.z = I_v[2];

    return odom;

}